#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <ServoSoft.h>
#include <MsTimer2.h>
#include <defArduinoParaArduino.h>

#define PULSES_PER_MM 130/499
#define PULSES_PER_DEG 52/90

#define IR_FD_PIN 3 //InfraRed Frontal Direito no ananolg 3
#define IR_FE_PIN 4
#define IR_TD_PIN 5
#define IR_TE_PIN 6
#define IR_MD_PIN 7
#define IR_ME_PIN 2

#define bitG 4 //digital 4
#define bitR 5
#define bitD 7
#define bitS 8

#define ENABLE 6

char msg[8];
char msgSize;
short int numSeqEsperado = 1;
short int executarTarefa = 0;
volatile short int reboot = LOW;
volatile short int esperaStart = HIGH;
volatile short int flag = LOW;
short int coco = 1;

volatile long int pulsosLidos = 0;



 Servo servo01; // traseira - esquerda
 Servo servo02; // traseira - direita
 Servo servo03; // frontal - direita 
 Servo servo04; // frontal - esquerda 


int serialGetMsg()
{
  int i;  
  i=0;
  
    while (Serial.available())
        msg[i++] = Serial.read();
    
  if(i>0)
  {
    msg[i]='\0';
    Serial.flush();
  }
  
  msgSize=i;
  
  return i;
}


 
void refreshServos()
{
  Servo::refresh();
  
  
  int n=0;
  int numSeqRecebido = -1;
  
  n = serialGetMsg();
  
  if( n )
  {
    if( (msg[0]=='<') && (msg[6]=='>') )
    {
      numSeqRecebido = (msg[5]-'0'); 
          
            if( numSeqRecebido == numSeqEsperado )
            {
              executarTarefa = 1;
            }
            else  
              Serial.println("NACK");  
     }
     else 
       if( strcmp(msg,"[START]") == 0 )
       { 
            numSeqEsperado = 1;
            executarTarefa = 0;
            Serial.flush();
            Serial.println("ACK");
            asm volatile ("jmp 0"); // reset via software =)
       }
       else
         Serial.println("NACK");
    
  }
 
 
}




void checkSerial()
{
  int n=0;
  int numSeqRecebido = -1;
  
  n = serialGetMsg();
  
  if( n )
  {
    if( (msg[0]=='<') && (msg[6]=='>') )
    {
      numSeqRecebido = (msg[5]-'0'); 
          
            if( numSeqRecebido == numSeqEsperado )
            {
              executarTarefa = 1;
            }
            else  
              Serial.println("NACK");  
     }
     else 
       if( strcmp(msg,"[START]") == 0 )
       { 
            numSeqEsperado = 1;
            Serial.println("ACK");
            asm volatile ("jmp 0"); // reset via software =)
       }
       else
         Serial.println("NACK");
    
  }
  
}


void sendMsg( int mensagem )
{
  char resposta[8],aux[4];
  aux[0]='\0';
  
  resposta[0] = '<';
  resposta[1] = '\0';

  sprintf(aux,"%d",mensagem); // transforma o int em string
    
  if ( strlen(aux) == 1 )
  {
    strcpy(resposta,"<000");
    strcat(resposta,aux);
  
  } else 
      if ( strlen(aux) == 2 )
      {
         strcpy(resposta,"<00");
         strcat(resposta,aux);
      }
      else 
        if( strlen(aux) == 3 )
        {
          strcpy(resposta,"<0");
          strcat(resposta,aux);
        }
        else
          strcat(resposta,aux);
        
 resposta[5] = '>';
 resposta[6] = '\0';

  Serial.println(resposta);
  
}


void avante()
{
    servo01.write(90);
    servo02.write(0);
    servo03.write(90);
    servo04.write(0);
    Servo::refresh();

    delay(1000);    
}


void aoLado()
{
    servo01.write(0);
    servo02.write(90);
    servo03.write(0);
    servo04.write(90);
    Servo::refresh();    

    delay(1000);
}


void girar()
{
    servo01.write(45);
    servo02.write(45);
    servo03.write(45);
    servo04.write(45);
    Servo::refresh();    
    
    delay(1000);
}


void stop()
{
  digitalWrite(bitG,0);
  digitalWrite(bitR,1);
  digitalWrite(bitD,1);
  digitalWrite(bitS,0);
  delay(500);
  
  digitalWrite(ENABLE,0);
}


void go_fwd(long int mm, int velocidade)
{
  long int pulsos = mm*(float)PULSES_PER_MM; // mm -> pulsos
  
  avante();
  pulsosLidos = 0;
  
  digitalWrite(bitG,0);
  digitalWrite(bitR,0);
  digitalWrite(bitD,0);
  digitalWrite(bitS,0);
  
  analogWrite(ENABLE,velocidade);
  
  if( mm != -1 )
  {
    while( pulsosLidos < pulsos )
    { delay(5); }
  
    stop();
  }
  
  pulsosLidos = 0;
}


void go_rev(long int mm, int velocidade)
{
  long int pulsos = mm*(float)PULSES_PER_MM;
  
  avante();
  pulsosLidos = 0;
  
  digitalWrite(bitG,0);
  digitalWrite(bitR,0);
  digitalWrite(bitD,0);
  digitalWrite(bitS,1);
  
  analogWrite(ENABLE,velocidade);
  
  if( mm != -1 )
  {
    while( pulsosLidos < pulsos )
    { delay(5); }
  
    stop();
  }
  
  pulsosLidos = 0;
  
}


void go_left(long int mm, int velocidade)
{
  long int pulsos = mm*(float)PULSES_PER_MM;
  
  aoLado();
  pulsosLidos = 0;
  
  digitalWrite(bitG,0);
  digitalWrite(bitR,0);
  digitalWrite(bitD,1);
  digitalWrite(bitS,0);
  
  analogWrite(ENABLE,velocidade);
   
  if( mm != -1 )
  {
    while( pulsosLidos < pulsos )
    { delay(5); }
  
    stop();
  }
  
  pulsosLidos = 0;
}


void go_right(long int mm, int velocidade)
{
  long int pulsos = mm*(float)PULSES_PER_MM;
  
  aoLado();
  pulsosLidos = 0;
  
  digitalWrite(bitG,0);
  digitalWrite(bitR,0);
  digitalWrite(bitD,1);
  digitalWrite(bitS,1);
  
  analogWrite(ENABLE,velocidade);
  
  if( mm != -1 )
  {
    while( pulsosLidos < pulsos )
    { delay(5); }
  
    stop();
  }
  
  pulsosLidos = 0;
}


void turn_ccw(long int angulo, int velocidade)
{
  long int pulsos = angulo*(float)PULSES_PER_DEG; // degrees -> pulsos
  girar();
  
  pulsosLidos = 0;
   
  digitalWrite(bitG,0);
  digitalWrite(bitR,1);
  digitalWrite(bitD,0);
  digitalWrite(bitS,1);
  
  analogWrite(ENABLE,velocidade);
  
  if( angulo != -1 )
  {
    while( pulsosLidos < pulsos )
    { delay(5); }
  
    stop();
  }
  
  pulsosLidos = 0;
  
}


void turn_cw(long int angulo, int velocidade)
{
  long int pulsos = angulo*(float)PULSES_PER_DEG;
  girar();
  
  pulsosLidos = 0;
  
  digitalWrite(bitG,0);
  digitalWrite(bitR,1);
  digitalWrite(bitD,0);
  digitalWrite(bitS,0);
  
  digitalWrite(ENABLE,velocidade);
  
  if( angulo != -1 )
  {
    while( pulsosLidos < pulsos )
    { delay(5); }
  
    stop();
  }
  
  pulsosLidos = 0;
  
}


void girarSobre( int qualServo, int intOUext, int velocidade )
{
  short int g,r,d,s;
  
  switch( qualServo )
  {
    case 1: servo02.write(0);
            servo03.write(45);
            servo04.write(90);
            Servo::refresh();
            delay(300);
            
            g = 1;
            r = 1;
            d = 0;
            s = ( intOUext==EXTERNO ? 0 : 1 );

    break;
    
    case 2: servo01.write(90);
            servo03.write(0);
            servo04.write(45);
            Servo::refresh();
            delay(300);
            
            g = 1;
            r = 1;
            d = 1;
            s = ( intOUext==EXTERNO ? 1 : 0 );
    break;  
    
    case 3: servo01.write(45);
            servo02.write(90);
            servo04.write(0);
            Servo::refresh();
            delay(300);
              
            g = 1;
            r = 0;
            d = 0;
            s = ( intOUext==EXTERNO ? 0 : 1 );
 
    break;
    
    case 4: servo01.write(0);
            servo02.write(45);
            servo03.write(90);
            Servo::refresh();
            delay(300);
            
            g = 1;
            r = 0;
            d = 1;
            s = ( intOUext==EXTERNO ? 1 : 0 );

    break;
    
     case 23: servo01.write(63);
              servo02.write(90);
              servo03.write(0);
              servo04.write(27);
              Servo::refresh();
              delay(300);
            
              g = 0;
              r = 1;
              d = 0;
              s = ( intOUext==EXTERNO ? 1 : 0 );
              
     break;
              
     
     case 14: servo01.write(0);
              servo02.write(27);
              servo03.write(63);
              servo04.write(90);
              Servo::refresh();
              delay(300);
            
              g = 0;
              r = 1;
              d = 0;
              s = ( intOUext==EXTERNO ? 0 : 1 );         

     break;
  
  }
  
  digitalWrite(bitG,g);
  digitalWrite(bitR,r);
  digitalWrite(bitD,d);
  digitalWrite(bitS,s);
  
  analogWrite(ENABLE,velocidade);

}



void alinharIR(int face, int intOUext, int a_velocidade )
{
  int IR1,TH1,IR2,TH2,PG1,PG2,IR1_val,IR2_val,S1,S2,faceFita; //,ajuste,flag=1;
  int velocidade = a_velocidade;
 
DENOVO:
    
    switch( face )
    {
      case FRENTE: IR1 = IR_FD_PIN;
                   TH1 = THRESHOLD_IR_FD; 
                   PG1 = 3; // Ponto de Giro sobre o servo3 
                   IR2 = IR_FE_PIN;
                   TH2 = THRESHOLD_IR_FE;
                   PG2 = 4;
                   faceFita = intOUext;
                   
                   avante();  
                   
                   if( intOUext == EXTERNO )
                     go_fwd(-1,velocidade);
                   else
                     go_rev(-1,velocidade);
                   
      break;
     
      case TRAS:   IR1 = IR_TD_PIN;
                   TH1 = THRESHOLD_IR_TD;
                   PG1 = 2;
                   IR2 = IR_TE_PIN;
                   TH2 = THRESHOLD_IR_TE;
                   PG2 = 1;
                   faceFita = intOUext;
                   
                   avante();
                   
                   if( intOUext == EXTERNO )
                     go_rev(-1,velocidade);
                   else
                     go_fwd(-1,velocidade);
                   
      break;
      
      case LAT_ESQ: IR1 = IR_FE_PIN;
                    TH1 = THRESHOLD_IR_FE;  
                    PG1 = 4;
                    IR2 = IR_TE_PIN;
                    TH2 = THRESHOLD_IR_TE;  
                    PG2 = 1;
                    faceFita = !intOUext;
                    
                    aoLado();
                    
                    if( intOUext == EXTERNO )
                     go_left(-1,velocidade);
                   else
                     go_right(-1,velocidade);
                   
      break;
      
      case LAT_DIR: IR1 = IR_FD_PIN;
                    TH1 = THRESHOLD_IR_FD;  
                    PG1 = 3;
                    IR2 = IR_TD_PIN;
                    TH2 = THRESHOLD_IR_TD;
                    PG2 = 2;
                    faceFita = !intOUext;
                    
                    aoLado();
                    
                    if( intOUext == EXTERNO )
                     go_right(-1,velocidade);
                   else
                     go_left(-1,velocidade);
                     
      break;
      
      case MEIO_DIR: IR1 = IR_FD_PIN;
                     TH1 = THRESHOLD_IR_FD;  
                     PG1 = 3;
                     IR2 = IR_MD_PIN;
                     TH2 = THRESHOLD_IR_MD;
                     PG2 = 23;
                     faceFita = !intOUext;
                     velocidade += 15; // akela merda de nao andar!
                    
                     aoLado();
                    
                     if( intOUext == EXTERNO )
                      go_right(-1,velocidade);
                    else
                      go_left(-1,velocidade);
      
      break;
      
      
      case MEIO_ESQ: IR1 = IR_FE_PIN;
                     TH1 = THRESHOLD_IR_FE;  
                     PG1 = 4;
                     IR2 = IR_ME_PIN;
                     TH2 = THRESHOLD_IR_ME;  
                     PG2 = 14;
                     faceFita = !intOUext;
                     velocidade += 15; // akela merda de nao andar!
                    
                     aoLado();
                    
                     if( intOUext == EXTERNO )
                      go_left(-1,velocidade);
                    else
                      go_right(-1,velocidade);
                   
      break;
      
      
      case MEIO:   IR1 = IR_MD_PIN;
                   TH1 = THRESHOLD_IR_MD; 
                   PG1 = 23; 
                   IR2 = IR_ME_PIN;
                   TH2 = THRESHOLD_IR_ME;
                   PG2 = 14;
                   faceFita = !intOUext;
                   velocidade += 15; // akela merda de nao andar!
                   
                   avante();  
                   
                   if( intOUext == EXTERNO )
                     go_fwd(-1,velocidade);
                   else
                     go_rev(-1,velocidade);
                     
      break;

    }
    
   while( ( (IR1_val = analogRead(IR1)) < TH1 ) && ( (IR2_val = analogRead(IR2)) < TH2 ) )
      delay(10);
    
    stop();
    
   while( ( (IR1_val = analogRead(IR1)) < TH1 ) || ( (IR2_val = analogRead(IR2)) < TH2 ) )
   {
     
     if( (IR1_val > TH1) && (IR2_val < TH2) )
      {
        if( analogRead(IR2) < TH2  )
          girarSobre( PG1, faceFita, velocidade );
      
        while( analogRead(IR2) < TH2  )
          delay(10);
        
        
        delay(50); // VEEEEEEEEEEEEEERRRRRRRR ISSO!
        stop();      
      }
      else if( (IR2_val > TH2) && (IR1_val < TH1) )
           { 
              if( analogRead(IR1) < TH1  )
                girarSobre( PG2, faceFita, velocidade );
              
              while( analogRead(IR1) < TH1  )
                delay(10);
         
              delay(50); // VEEEEEEEEEEEEEERRRRRRRR ISSO!    
                stop();  
           }
           else if( (IR2_val < TH2) && (IR1_val < TH1) )
                  goto DENOVO; // dois sensores fora da fita denovo (improvavel, mas tem q considerar..)
                else
                 Serial.println( "nenhum dos casos? =O" ); // DEU MERDA!
   }
   
   
   stop();
   
/*   if( (face == FRENTE) || (face == TRAS) )
     avante();
   else
     aoLado();  */

}

 
void leSensores()
{

  char sensorId[4];
  int sensorID;
  
  sensorId[0] = msg[1];
  sensorId[1] = msg[2];
  sensorId[2] = msg[3];
  sensorId[3] = '\0';
  
  sensorID = atoi(sensorId);
  
  int leitura = sensorID;

  switch( sensorID )
  {
    case IR_FD: leitura = analogRead(IR_FD_PIN);
                break;
    
    case IR_FE: leitura = analogRead(IR_FE_PIN);
                break;
    
    case IR_TD: leitura = analogRead(IR_TD_PIN);
                break;
    
    case IR_TE: leitura = analogRead(IR_TE_PIN);
                break;
                
    case IR_MD: leitura = analogRead(IR_MD_PIN);
                break;
                
    case IR_ME: leitura = analogRead(IR_ME_PIN);
                break;
    
  }

  sendMsg( leitura );
  
}



int pegaInstrucao()
{

  long int arg = 0,pulses_mm,pulses_deg;  

  arg = (msg[1] - '0')*100;
  arg = arg + (msg[2] - '0')*10;
  arg = arg + (msg[3] - '0')*1;

  if( arg < 0 || arg > 999 ) // soh pra garantir
    return -1;

      switch( msg[4] )
      {
        case OMNI_FWD: go_fwd(arg,255);
                       break;
        
        case OMNI_REV: go_rev(arg,255);
                       break;
                      
        case OMNI_DIR: go_right(arg,255);
                       break;
                  
        case OMNI_ESQ: go_left(arg,255);
                       break;
                                    
        case OMNI_CCW: turn_ccw(arg,255);
                       break;
                  
       case OMNI_CW: turn_cw(arg,255);
                     break;

       case OMNI_ALINHA_IR: if( ((msg[2]-'0') < 0) || ((msg[2]-'0') > 7) )
                              return -1;
                            else  
                              alinharIR( (msg[2]-'0'),(msg[3]-'0'),60); // transforma em int
                              
                            break;
       
       case LE_SENSORES: leSensores();
                         break; 

       default : return -1;
      
      }

return 0;
}

void encoder()
{
  pulsosLidos++;
}


void setup() 
{ 
  
  MsTimer2::set(20,refreshServos);
  //MsTimer2::set(37,checkSerial);
  Serial.begin(115200);
  
  attachInterrupt(0, encoder, RISING); // digital 2
  //attachInterrupt(0, startOUreboot, RISING); // digital 3
  
  pinMode(ENABLE,OUTPUT);
  
  pinMode(bitG,OUTPUT);
  pinMode(bitR,OUTPUT);
  pinMode(bitD,OUTPUT);
  pinMode(bitS,OUTPUT);
  
  analogWrite(ENABLE,0);
  
  digitalWrite(bitG,0);
  digitalWrite(bitR,0);
  digitalWrite(bitD,0);
  digitalWrite(bitS,0);
  

/* --- Configuracao para os servos MG946R */

  servo01.attach(9); 
  servo01.setMinimumPulse(830);
  servo01.setMaximumPulse(2680);

  servo02.attach(10); 
  servo02.setMinimumPulse(780);
  servo02.setMaximumPulse(2680);

  servo03.attach(11); 
  servo03.setMinimumPulse(890);
  servo03.setMaximumPulse(2830);
  
  servo04.attach(12); 
  servo04.setMinimumPulse(870);
  servo04.setMaximumPulse(2850);
    
  MsTimer2::start();
  delay(100);
  //avante();
} 



void loop() 
{  
  
 /* while(1)
  {
    Serial.println(analogRead(IR_FD_PIN));
    Serial.println(analogRead(IR_MD_PIN));
    Serial.println(analogRead(IR_TD_PIN));
    Serial.println(analogRead(IR_FE_PIN));
    Serial.println(analogRead(IR_ME_PIN));
    Serial.println(analogRead(IR_TE_PIN));
    delay(1000);
  }*/
  
  
  //while( true )
  //{
  
    delay(10);
    
    if( executarTarefa == 1 )            
    {
      if( pegaInstrucao( ) == 0 ) // executa
      {
        Serial.println("ACK");
                 
        if( numSeqEsperado == 9)
          numSeqEsperado = 1;
        else
          numSeqEsperado++;
      }
      else
        Serial.println("NACK");

    executarTarefa = 0;        
    }
  
 // delay(5);  
  //}
  
    
}
